dx = 1.202;
dy = 0.495;
da = 0.349;

ref = load('refScan.txt');
act = load('actScan.txt');

figure(100);
hold on;
title('Two scans plotted in polar coordinates');
plot(ref(:,1),'or');
plot(act(:,1),'ob');
print -dpng scans-polar-coordinates.png 
'Press a key to continue'
%pause
close all;


rx = ref(:,1).*cos(ref(:,2));
ry = ref(:,1).*sin(ref(:,2));

ax = act(:,1).*cos(act(:,2));
ay = act(:,1).*sin(act(:,2));

figure(1);
hold on;
axis('equal');
title('Two scans plotted in the reference coordinates');
plot(rx,ry,'or');
plot(ax,ay,'ob');
print -dpng scans-ref-coordinates.png 

ax = act(:,1).*cos(act(:,2)+da);
ay = act(:,1).*sin(act(:,2)+da);

close all;
figure(2);
hold on;
axis('equal');
title('Heading error corrected');
plot(rx,ry,'or');
plot(ax,ay,'ob');
print -dpng scans-ref-heading-corrected.png 

ax = act(:,1).*cos(act(:,2)+da)-dy;
ay = act(:,1).*sin(act(:,2)+da)+dx;

close all;
figure(3);
hold on;
axis('equal');
title('All corrected');
plot(rx,ry,'or');
plot(ax,ay,'ob');
print -dpng scans-ref-all-corrected.png 